#include "modules/CtrlModule/MarkPosePubModel.h"
#include "modules/AssistModule/LogModule.h"


namespace behavior_controller
{
MarkPosePubModel mark_pose_pub_mod;

MarkPosePubModel::MarkPosePubModel(const rclcpp::Node::SharedPtr &ros_node)
        : ros_node_(ros_node)
{
    mark_pose_pub_ = ros_node_->create_publisher<geometry_msgs::msg::PoseArray>("mark_pose_array", 1);
    LogPub::Info("behavior_controller", "[MarkPosePubModel] .........MarkPosePubModel::MarkPosePubModel");
}

void MarkPosePubModel::SetNode(const rclcpp::Node::SharedPtr &ros_node)
{
    if (mark_pose_pub_ == nullptr)
    {
        ros_node_ = ros_node;
        mark_pose_pub_ = ros_node_->create_publisher<geometry_msgs::msg::PoseArray>("mark_pose_array", 1);
        LogPub::Info("behavior_controller", "[MarkPosePubModel] .........MarkPosePubModel::SetNode");
    }


};

void MarkPosePubModel::MarkPosePub(std::vector<PointF> &pose_array)
{
    geometry_msgs::msg::PoseArray pose_array_tmp;
    geometry_msgs::msg::Pose pose_tmp;
    pose_array_tmp.header.frame_id = "footprint_frame";
    for (auto & i : pose_array)
    {
        pose_tmp.position.x = i.x();
        pose_tmp.position.y = i.y();
        pose_array_tmp.poses.push_back(pose_tmp);
    }
    mark_pose_pub_->publish(pose_array_tmp);
}

}
